#include <boost/date_time/posix_time/posix_time.hpp>
#include <fstream>
#include <geometry_msgs/PoseStamped.h>
#include <queue>
#include <ros/ros.h>
#include <string>
std::queue<geometry_msgs::PoseStamped> gDataBufferVicon;
std::queue<geometry_msgs::PoseStamped> gDataBufferNokov;
void VrpnCallback(geometry_msgs::PoseStampedConstPtr msg) {
  gDataBufferVicon.push(*msg);
}
void VrpnNokovCallback(geometry_msgs::PoseStampedConstPtr msg) {
  gDataBufferNokov.push(*msg);
}
std::string TimeToStr(const ros::Time &ros_t) {
  (void)ros::WallTime::now();
  std::stringstream msg;
  const boost::posix_time::ptime now =
      boost::posix_time::second_clock::local_time();
  ;
  boost::posix_time::time_facet *const f =
      new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S");
  msg.imbue(std::locale(msg.getloc(), f));
  msg << now;
  return msg.str();
}
int main(int argc, char **argv) {
  ros::init(argc, argv, "record_node");
  ros::NodeHandle nh("~");
  ros::Subscriber poseSub =
      nh.subscribe("/vrpn_client_node/DroneRig/pose", 10000, VrpnCallback,
                   ros::TransportHints().tcpNoDelay());
  ros::Rate r(100);
  std::string savePathPrefix;
  if (argc < 2) {
    ROS_ERROR("Usage: rosrun record_drone_vrpn record_drone_vrpn_node "
              "savePathPrefix");
    return -1;
  }
  savePathPrefix = argv[1];
  std::string savePathVicon;
  std::string savePathVNokov;
  std::string startTime = TimeToStr(ros::Time::now());
  savePathVicon = savePathPrefix + startTime + "_vicon" + ".txt";
  savePathVNokov = savePathPrefix + startTime + "_nokov" + ".txt";
  ROS_INFO("Result save into: %s\n.", savePathVicon.c_str());
  std::ofstream outFileVicon;
  outFileVicon.open(savePathVicon);
  ROS_ASSERT(outFileVicon.is_open());
  outFileVicon << "timestamp, "
                  "p_RS_R_x[m],p_RS_R_y[m],p_RS_R_z[m],q_RS_w[],q_RS_x[],q_RS_"
                  "y[],q_RS_z[]"
               << std::endl;
  while (ros::ok()) {
    while (!gDataBufferVicon.empty()) {
      double timestamp = gDataBufferVicon.front().header.stamp.toNSec();
      double pX = gDataBufferVicon.front().pose.position.x;
      double pY = gDataBufferVicon.front().pose.position.y;
      double pZ = gDataBufferVicon.front().pose.position.z;
      double qW = gDataBufferVicon.front().pose.orientation.w;
      double qX = gDataBufferVicon.front().pose.orientation.x;
      double qY = gDataBufferVicon.front().pose.orientation.y;
      double qZ = gDataBufferVicon.front().pose.orientation.z;
      outFileVicon << std::setprecision(20) << timestamp << ","
                   << std::setprecision(10) << pX << "," << pY << "," << pZ
                   << "," << qW << "," << qX << "," << qY << "," << qZ
                   << std::endl;
      gDataBufferVicon.pop();
    }
    r.sleep();
    ros::spinOnce();
  }
}
